#include "hidjoystickrptparser.h"
#include "config.h"

int PPM_array[9];

uint16_t JoyX;
uint16_t JoyY;
uint16_t JoyZ;
int16_t TrimElev = 0;


void recalculatePPM() {
  PPM_array[0] = map(constrain(JoyY, 1000, 2000), 1000, 2000, pulseMin, pulseMax); // Channel 1: Aileron  
  PPM_array[1] = map(constrain(JoyX, 1000, 2000), 1000, 2000, pulseMin, pulseMax); // Channel 2: Elevator
  PPM_array[2] = map(constrain(JoyZ, 1000, 2000), 1000, 2000, pulseMin, pulseMax); // Channel 3: Throttle

}

JoystickReportParser::JoystickReportParser(JoystickEvents *evt) : 
	joyEvents(evt),
	oldButtons(0)
{
	for (uint8_t i=0; i<RPT_GEMEPAD_LEN; i++)
		oldPad[i]	= 0xD; 
}

void JoystickReportParser::Parse(HID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf)
{
	bool match = true;

	// Checking if there are changes in report since the method was last called
	for (uint8_t i=0; i<RPT_GEMEPAD_LEN; i++)
		if (buf[i] != oldPad[i])
		{
			match = false;
			break;
		}

	// Calling Game Pad event handler
	if (!match && joyEvents)
	{
		joyEvents->OnGamePadChanged((const GamePadEventData*)buf);

		for (uint8_t i=0; i<RPT_GEMEPAD_LEN; i++) oldPad[i] = buf[i];
	}


	uint8_t buttons = (0x00 | buf[3]);
	//buttons <<= 0;
	//buttons |= (buf[5] >> 0);
	uint8_t changes = (buttons ^ oldButtons);

	// Calling Button Event Handler for every button changed
	if (changes)
	{
		for (uint8_t i=0; i<0x06; i++)
		{
			uint16_t mask = (0x0001 << i);

			if (((mask & changes) > 0) && joyEvents)
				if ((buttons & mask) > 0)
					joyEvents->OnButtonDn(i+1);
				else
					joyEvents->OnButtonUp(i+1);
		}
		oldButtons = buttons;
	}
}

void JoystickEvents::OnGamePadChanged(const GamePadEventData *evt)
{
  JoyX = map((evt->X), 0, 255, 1000, 2000); //Left/right
  JoyY = map((evt->Y), 255, 0, 1000, 2000); //For-/backwards
  JoyZ = map((evt->Z), 255, 0, 1000, 2000); //Throttle
  
  recalculatePPM();
  
  Serial.print("JoyX: ");
  Serial.print(JoyX);
  Serial.print(" JoyY: ");
  Serial.print(JoyY);
  Serial.print(" JoyZ: ");
  Serial.print(JoyZ);
  Serial.println("");
}

void JoystickEvents::OnButtonUp(uint8_t but_id)
{
  //nothing to do  
}

void JoystickEvents::OnButtonDn(uint8_t but_id)
{
  
  if(but_id == 2) {  //+Button
    if(TrimElev >= (TrimMax - TrimStep)) {
      //we are at Max now (or arriving it)
      TrimElev = TrimMax;
      //Beep
    }
    else {
      TrimElev += TrimStep;
    }

    if(TrimElev==0) {
      //Beep
    }
  }

  if(but_id == 3) { //-Button
    if(TrimElev <= (TrimStep-TrimMax)) {
      //we are at Min now (or arriving it)
      TrimElev = 0 - TrimMax;
      //Beep
    }
    else {
      TrimElev -= TrimStep;
    }

    if(TrimElev==0) {
      //Beep
    }
  }
  
  recalculatePPM();
    
  Serial.print("TrimElev: ");
  Serial.println(TrimElev);
  Serial.print("Dn: ");
  Serial.println(but_id, DEC);
  
}
